#include <Arduino.h>
#include <ESP32Servo.h>   // 包含 ESP32Servo 库
#include <Adafruit_NeoPixel.h> // 包含 Adafruit_NeoPixel 库
#include <Preferences.h>

#include "wexcube_sdk/wexcube.h"
#include "wexcube_sdk/wexcube_port.h"

// BLE 名称前缀
#define BLE_NAME_PRE        "WEX-RobotDog-"

// 小程序设备控制页面控件ID
#define BUTTON_EYES_COLOR1_ID   1   // 眼睛颜色1
#define BUTTON_EYES_COLOR2_ID   2   // 眼睛颜色2
#define BUTTON_EYES_COLOR3_ID   3   // 眼睛颜色3
#define BUTTON_EYES_COLOR4_ID   4   // 眼睛颜色4
#define BUTTON_EYES_COLOR5_ID   5   // 眼睛颜色5
#define BUTTON_EYES_COLOR6_ID   6   // 眼睛颜色6
#define SLIDER_EYES_LEVEL_ID    7   // 眼睛亮度
#define BUTTON_EYES_MODE1_ID    8   // 眼睛模式1
#define BUTTON_EYES_MODE2_ID    9   // 眼睛模式2
#define BUTTON_EYES_MODE3_ID    10  // 眼睛模式3 

#define SLIDER_LEFTHAND_ID      11  // 左手舵机
#define SLIDER_RIGHTHAND_ID     12  // 右手舵机
#define SLIDER_LEFTLEG_ID       13  // 左脚舵机
#define SLIDER_RIGHTLEG_ID      14  // 右脚舵机  

#define BUTTON_FORWARD_ID       15  // 前进
#define BUTTON_LEFT_ID          16  // 左转
#define BUTTON_RIGHT_ID         17  // 右转
#define BUTTON_BACKWARD_ID      18  // 后退  

#define BUTTON_STANDUP_ID       19  // 站立
#define BUTTON_GETDOWN_ID       20  // 趴下
#define BUTTON_LEFTRIGHT_ID     21  // 左右摇晃
#define BUTTON_FRONTBACK_ID     22  // 前后摇晃
#define BUTTON_SITDOWN_ID       24  // 坐下
#define BUTTON_STRETCH_ID       25  // 伸懒腰
#define BUTTON_HANDSHAKE_ID     26  // 握手
#define BUTTON_CRAWL_ID         27  // 匍匐前进
#define BUTTON_HEADFWD_ID       41  // 前顶
#define BUTTON_HEADBACK_ID      42  // 后顶

#define BUTTON_LEFTHAND_MINUS_ID  28  // 左手舵机角度减小
#define BUTTON_LEFTHAND_PLUS_ID   29  // 左手舵机角度增大
#define BUTTON_RIGHTHAND_MINUS_ID 30  // 右手舵机角度减小
#define BUTTON_RIGHTHAND_PLUS_ID  31  // 右手舵机角度增大
#define BUTTON_LEFTLEG_MINUS_ID   32  // 左脚舵机角度减小
#define BUTTON_LEFTLEG_PLUS_ID    33  // 左脚舵机角度增大
#define BUTTON_RIGHTLEG_MINUS_ID  34  // 右脚舵机角度减小
#define BUTTON_RIGHTLEG_PLUS_ID   35  // 右脚舵机角度增大

#define BUTTON_CALI_SET_ID        36  // 校准写入
#define BUTTON_CALI_RESET_ID      37  // 校准复位

#define SLIDER_MOVE_SPEED_ID    23  // 动作速度

#define TEXT_VERSION_ID         38  // 固件版本号
#define TEXT_BATTERY_ID         39  // 电池状态
#define TEXT_HARDWARE_ID        40  // 硬件版本号


// 引脚定义
#define WEX_LED_PIN             8   // WeXCube 指示灯引脚
#define EYES_CTRL_PIN           6   // 眼睛控制引脚
#define LEFTHAND_CTRL_PIN       0   // 左手舵机控制引脚
#define RIGHTHAND_CTRL_PIN      1   // 右手舵机控制引脚
#define LEFTLEG_CTRL_PIN        10  // 左脚舵机控制引脚
#define RIGHTLEG_CTRL_PIN       7   // 右脚舵机控制引脚
#define TOUCH_SW_PIN            4   // 触摸按键引脚
#define RX_PIN                  2   // 串口接收引脚
#define TX_PIN                  3   // 串口发送引脚

#define EYES_NUM                2   // 眼睛灯带数量

#define THERSHOLD_ANGLE         65  // 舵机角度变化阈值


// 灯光模式枚举
enum lightModel_t
{
  LIGHT_MODEL_NORMAL = 0,   // 正常模式
  LIGHT_MODEL_BREATHING,    // 呼吸模式
  LIGHT_MODEL_COLORFUL      // 彩灯模式
};

// 动作模式枚举
enum actionModel_t
{
  ACTION_MODEL_NONE = 0,      // 无动作
  ACTION_MODEL_FORWARD,       // 向前
  ACTION_MODEL_LEFT,          // 向左
  ACTION_MODEL_RIGHT,         // 向右
  ACTION_MODEL_BACKWARD,      // 后退
  ACTION_MODEL_STANDUP,       // 站立
  ACTION_MODEL_GETDOWN,       // 趴下
  ACTION_MODEL_SITDOWN,       // 坐下
  ACTION_MODEL_STRETCH,       // 伸懒腰动作
  ACTION_MODEL_HANDSHAKE,     // 握手动作
  ACTION_MODEL_CRAWL,         // 匍匐前进
  ACTION_MODEL_LEFTRIGHT,     // 左右摇晃
  ACTION_MODEL_FRONTBACK,     // 前后摇晃
  ACTION_MODEL_HEADFWD,       // 前顶
  ACTION_MODEL_HEADBACK,      // 后顶
};

// 状态枚举
enum stateModel_t
{
  STATE_MODEL_IDLE = 0,       // 空闲状态
  STATE_MODEL_MOVING,         // 移动状态
  STATE_MODEL_STANDUP,        // 站立状态
  STATE_MODEL_GETDOWN,        // 趴下状态
  STATE_MODEL_SITDOWN         // 坐下状态
};

Servo leftHandServo;                // 左手舵机对象
Servo rightHandServo;               // 右手舵机对象
Servo leftLegServo;                 // 左脚舵机对象
Servo rightLegServo;                // 右脚舵机对象
int leftHandAngle = 90;             // 左手舵机角度
int rightHandAngle = 90;            // 右手舵机角度
int leftLegAngle = 90;              // 左脚舵机角度
int rightLegAngle = 90;             // 右脚舵机角度
wex_u8_t actionModel = 0;           // 动作模式（enum actionModel_t）
wex_u8_t stateModel = 0;            // 状态模式（enum stateModel_t）
wex_u8_t moveSpeed = 6;             // 动作速度（2~10）
wex_u8_t stepFlag = 0;

Adafruit_NeoPixel strip(EYES_NUM, EYES_CTRL_PIN, NEO_GRB + NEO_KHZ800);

wex_u8_t lightLevel = 0;  // 0 ~ 100
wex_u8_t lightColor = 0;
wex_u8_t lightModel = 0; // 0 - 正常，1 - 呼吸，2 - 彩灯
wex_u8_t ledRGB[3] = {255, 255, 255}; // 眼睛灯光颜色（RGB）

wex_u8_t askHandshakeCnt = 0;       // 心跳包计数
bool wexConnectFlag = false;        // WeXCube 连接标志
bool wexcubeCtrlFlag = false;       // WeXCube 控制标志
wex_u8_t currentButtonId = 0;       // 当前按下去的按键 ID，0 表示没有按下去的按键

hw_timer_t *timer = NULL;           // 定时器对象
volatile bool timerFlag = false;    // 共享变量（标志位）
volatile int16_t dischargePulseCnt = 0;
volatile int16_t chargePulseCnt = 0;
const char *pcBatteryStatusStr[6] = {"未知状态", "电池放电中", "低电预警", "电量耗尽", "电池充电中", "电池已充满"}; // 电池状态字符串
wex_u8_t batteryStatus = 0; // 电池状态（0 - 未知状态，1 - 正在放电，2 - 低电量，3 - 电量耗尽， 4 - 正在充电， 5 - 充电完成）

wex_u32_t firmwareVersion = 0x010102; // 固件版本号（0x010100 = 1.1.2）
wex_u32_t hardwareVersion = 0x0000; // 硬件版本号

// 舵机角度偏移量，值范围-10~10，从存储中获取，默认为 0 
wex_s8_t leftHandOffset   = 0;  // 左手舵机偏移量，正值往后偏，负值往前偏
wex_s8_t rightHandOffset  = 0;  // 右手舵机偏移量，正值往前偏，负值往后偏
wex_s8_t leftLegOffset    = 0;  // 左脚舵机偏移量，正值往后偏，负值往前偏
wex_s8_t rightLegOffset   = 0;  // 右脚舵机偏移量，正值往前偏，负值往后偏
Preferences preferences;

volatile wex_u32_t secondCnt = 0;
void IRAM_ATTR onTimer() {
  timerFlag = true;
  secondCnt++;
  // 每2秒执行一次
  if (secondCnt % 2 == 0) {
    if (dischargePulseCnt > 10) {
      if (dischargePulseCnt < 1400) {
        batteryStatus = 2; // 低电量
      } else {
        batteryStatus = 1; // 放电
      }
    } else if (chargePulseCnt > 10) {
      if (chargePulseCnt < 1400) {
        batteryStatus = 4; // 充电
      } else {
        batteryStatus = 5; // 正常放电
      }
    } else {
      batteryStatus = 3; // 电量耗尽
    }
    dischargePulseCnt = 0;
    chargePulseCnt = 0;
  }
}

void IRAM_ATTR dischargePulseISR() {
  dischargePulseCnt++;
}

void IRAM_ATTR chargePulseISR() {
  chargePulseCnt++;
}

void wex_ble_loop();
void updateRGB(wex_u8_t ucRed, wex_u8_t ucGreen, wex_u8_t ucBlue, wex_u8_t ucLevel);
void updateServo(wex_u8_t ucLeftHand, wex_u8_t ucRightHand, wex_u8_t ucLeftLeg, wex_u8_t ucRightLeg);
void selectEyesColor(wex_u8_t id); // 选择眼睛颜色
void selectEyesModel(wex_u8_t id); // 选择眼睛模式
void standUp(); // 站立
void getDown(); // 趴下
void sitDown(); // 坐下
void stretch(); // 伸懒腰
void headForward(); // 前顶
void headBackward();  // 后顶

void saveOffsetsToFlash(); // 保存舵机偏移量到Flash


void setup() {
  // 降低到 80MHz
  setCpuFrequencyMhz(80);

  // 延时一段时间等待稳定
  delay(500);

  // 启动 BLE 设备
  Serial.begin(9600);

  // 开启串口：波特率 9600，8N1 格式，指定 RX/TX
  Serial1.begin(9600, SERIAL_8N1, RX_PIN, TX_PIN);

  // 启动 BLE 服务
  uint64_t chipid = ESP.getEfuseMac();
  char macStr[20];  
  sprintf(macStr, "%02x%02x%02x%02x%02x%02x", 
                  (uint8_t)(chipid >> 0),
                  (uint8_t)(chipid >> 8),
                  (uint8_t)(chipid >> 16),
                  (uint8_t)(chipid >> 24),
                  (uint8_t)(chipid >> 32),
                  (uint8_t)(chipid >> 40));
  wex_ble_config(BLE_NAME_PRE + String(macStr).substring(6));
  wex_init();
  wex_start();
  Serial.println("WeXCube start");

  // WeXCube 指示灯
  pinMode(WEX_LED_PIN, OUTPUT);
  digitalWrite(WEX_LED_PIN, HIGH);

  // 初始化定时器 (定时器编号 0, 分频系数 80, 递增模式)
  timer = timerBegin(0, 80, true);  // 80MHz / 80 = 1MHz（1us）
  // 绑定定时器中断回调
  timerAttachInterrupt(timer, &onTimer, true);
  // 设置定时器（1000000us = 1秒，重复触发）
  timerAlarmWrite(timer, 1000000, true);
  // 启动定时器
  timerAlarmEnable(timer);
  
  // 设置 PWM 输出频率为 50Hz（标准舵机控制频率，周期为20ms）
  leftHandServo.setPeriodHertz(50);
  rightHandServo.setPeriodHertz(50);
  leftLegServo.setPeriodHertz(50);
  rightLegServo.setPeriodHertz(50);
  
  // 附加舵机到指定引脚，并设置脉宽范围（500us ~ 2500us）
  // 500us ~ 2500us 对应舵机角度范围 0° ~ 180°（具体范围根据舵机型号和实际情况调整）
  leftHandServo.attach(LEFTHAND_CTRL_PIN, 500, 2500);
  rightHandServo.attach(RIGHTHAND_CTRL_PIN, 500, 2500);
  leftLegServo.attach(LEFTLEG_CTRL_PIN, 500, 2500);
  rightLegServo.attach(RIGHTLEG_CTRL_PIN, 500, 2500);

  // 从Flash存储中获取舵机校准偏移量
  preferences.begin("servo", false);
  leftHandOffset = preferences.getInt("leftHandOffset", 0);
  rightHandOffset = preferences.getInt("rightHandOffset", 0);
  leftLegOffset = preferences.getInt("leftLegOffset", 0);
  rightLegOffset = preferences.getInt("rightLegOffset", 0);
  preferences.end();

  // 设置舵机初始角度
  updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
  actionModel = ACTION_MODEL_NONE;

  strip.begin();
  strip.setBrightness(lightLevel * 2.55f); // 亮度初始值
  strip.show();           // 清空灯带

  // 触摸按键控制引脚初始化
  pinMode(TOUCH_SW_PIN, INPUT_PULLDOWN); // 触摸按键引脚设置为上拉输入

  // 获取硬件版本号
  pinMode(20, INPUT_PULLUP);
  pinMode(21, INPUT_PULLUP);
  delay(100); // 等待引脚稳定
  // 读取引脚值
  wex_u8_t pin20Value1 = digitalRead(20);
  wex_u8_t pin21Value1 = digitalRead(21);
  pinMode(20, INPUT_PULLDOWN);
  pinMode(21, INPUT_PULLDOWN);
  delay(100); // 等待引脚稳定
  // 读取引脚值
  wex_u8_t pin20Value2 = digitalRead(20);
  wex_u8_t pin21Value2 = digitalRead(21);
  if (pin20Value1 == HIGH && pin21Value1 == HIGH &&
      pin20Value2 == LOW && pin21Value2 == LOW) {
    hardwareVersion = 0x0100; // 硬件版本号 1.0
  } else {
    hardwareVersion = 0x0101; // 硬件版本号 1.1
  }


  pinMode(5, OUTPUT);
  digitalWrite(5, LOW);

  // 脉冲计数中断注册
  attachInterrupt(digitalPinToInterrupt(20), chargePulseISR, RISING);
  attachInterrupt(digitalPinToInterrupt(21), dischargePulseISR, RISING);
}

void loop() {
  wex_ble_loop();

  // 把调试口接收的数据转发到 WeXCube 协议栈
  while (Serial.available()) {
    char c = Serial.read();
    wex_push((wex_u8_t *)&c, 1);
    //Serial.printf("%02x ", c);  // 打印接收到的数据
  }

  // 把扩展口接收的数据转发到 WeXCube 协议栈
  while (Serial1.available()) {
    char c = Serial1.read();
    wex_push((wex_u8_t *)&c, 1);
    //Serial.printf("%02x ", c);  // 打印接收到的数据
  }

  // 延时 20ms，避免蓝牙通信阻塞
  delay(20);

  if (wexcubeCtrlFlag == false)
  {
    // 触摸事件处理：
    // 1. 单击，眼睛灯光开关
    // 2. 双击，站立/趴下
    int SW = digitalRead(TOUCH_SW_PIN); // 读取触摸按键值

    if (SW == HIGH) // 触摸按键按下
    {
      wex_u32_t i = 0;
      delay(30); // 防抖动延时

      // 等待摇杆按键松开
      while (digitalRead(TOUCH_SW_PIN) == HIGH)
      {
        i++;
        delay(10); 
      }
      if (i < 50)
      {
        while (i < 50)
        {
          delay(10);
          if (digitalRead(TOUCH_SW_PIN) == HIGH) // 触摸按键再次按下
            break;
          i++;
        }
      }
      if (i < 50)
      {
        if (leftLegAngle >= 70)
          actionModel = ACTION_MODEL_GETDOWN;
        else
          actionModel = ACTION_MODEL_STANDUP;
        delay(500); // 等待手拿开
      }
      else
      {
        lightLevel = lightLevel == 0 ? 50 : 0; // 切换眼睛灯光开关
        if (lightLevel == 0)
        {
          updateRGB(0, 0, 0, 0); // 关闭灯光
          wex_setValue(SLIDER_EYES_LEVEL_ID, 0); // 更新小程序滑块值
        }
        else
        {
          updateRGB(ledRGB[0], ledRGB[1], ledRGB[2], lightLevel); // 打开灯光
          wex_setValue(SLIDER_EYES_LEVEL_ID, lightLevel); // 更新小程序滑块值
        }
      }
    } 
  }

  // 眼睛灯光控制
  if (lightLevel)
  {
    if (lightModel == LIGHT_MODEL_NORMAL) // 正常模式
    {
      static wex_u32_t cnt = 0;
      cnt++;
      // 10 * 10 = 100ms 更新一次值
      if (cnt >= 10)
      {
        cnt = 0;
        updateRGB(ledRGB[0], ledRGB[1], ledRGB[2], lightLevel);
      }
    }
    else if (lightModel == LIGHT_MODEL_BREATHING) // 呼吸模式
    {
      static wex_u8_t level = 0;
      static wex_u8_t dir = 0;
      static wex_f32_t s = 0.0f;
      wex_f32_t v = (wex_f32_t)lightLevel / 50.0f;
      if (dir)
      {
        if (level >= lightLevel)
        {
          dir = 0;
          s = (wex_f32_t)lightLevel;
        }
        else
        {
          s += v;
          level = s < (wex_f32_t)lightLevel ? (wex_u8_t)s : lightLevel; // 确保不会超过lightLevel
        }
      }
      else
      {
        if (level == 0)
        {
          dir = 1;
          s = 0.0f;
        }
        else
        {
          s -= v;
          level = s > 0.0f ? (wex_u8_t)s : 0; // 确保不会小于0
        }
      }
      updateRGB(ledRGB[0], ledRGB[1], ledRGB[2], level);
    }
    else if (lightModel == LIGHT_MODEL_COLORFUL) // 彩灯模式
    {
      static long firstPixelHue = 0;

      // 彩虹渐变效果
      firstPixelHue += 256;
      if (firstPixelHue >= 65536)
        firstPixelHue = 0;
      for (int i = 0; i < strip.numPixels(); i++)
      {
        int pixelHue = firstPixelHue + (i * 65536L / strip.numPixels());
        strip.setPixelColor(i, strip.gamma32(strip.ColorHSV(pixelHue)));
      }
      strip.show();
    }
  }

  // 舵机动作控制
  switch (actionModel)
  {
    case ACTION_MODEL_FORWARD: // 前进
    {
      if (stepFlag == 0)
      {
        leftHandAngle -= moveSpeed;
        rightLegAngle -= moveSpeed;
        
        if (leftHandAngle <= THERSHOLD_ANGLE) stepFlag = 1;
      }
      else if (stepFlag == 1)
      {
        rightHandAngle += moveSpeed;
        leftLegAngle += moveSpeed;
        
        if (rightHandAngle >= (180 - THERSHOLD_ANGLE)) stepFlag = 2;
      }
      else if (stepFlag == 2)
      {
        leftHandAngle += moveSpeed;
        rightLegAngle += moveSpeed;
        
        if (leftHandAngle >= 90) stepFlag = 3;
      }
      else if (stepFlag == 3)
      {
        rightHandAngle -= moveSpeed;
        leftLegAngle -= moveSpeed;
        
        if (rightHandAngle <= THERSHOLD_ANGLE) stepFlag = 4;
      }
      else if (stepFlag == 4)
      {
        leftHandAngle += moveSpeed;
        rightLegAngle += moveSpeed;
        
        if (leftHandAngle >= (180 - THERSHOLD_ANGLE)) stepFlag = 5;
      }
      else if (stepFlag == 5)
      {
        rightHandAngle += moveSpeed;
        leftLegAngle += moveSpeed;
        
        if (rightHandAngle >= 90) stepFlag = 6;
      }
      else if (stepFlag == 6)
      {
        leftHandAngle -= moveSpeed;
        rightLegAngle -= moveSpeed;
        
        if (leftHandAngle <= 90) stepFlag = 0;
      }
      updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度

      stateModel = STATE_MODEL_MOVING; // 设置状态为移动
    }
    break;

    case ACTION_MODEL_LEFT: // 左转
    {
      if (stepFlag == 0)
      {
        rightHandAngle -= moveSpeed;
        leftLegAngle += moveSpeed;
        
        if (rightHandAngle <= THERSHOLD_ANGLE) stepFlag = 1;
      }
      else if (stepFlag == 1)
      {
        rightLegAngle += moveSpeed;
        leftHandAngle -= moveSpeed;
        
        if (rightLegAngle >= (180 - THERSHOLD_ANGLE)) stepFlag = 2;
      }
      else if (stepFlag == 2)
      {
        rightHandAngle += moveSpeed;
        leftLegAngle -= moveSpeed;
        
        if (rightHandAngle >= 90) stepFlag = 3;
      }
      else if (stepFlag == 3)
      {
        rightLegAngle -= moveSpeed;
        leftHandAngle += moveSpeed;
        
        if (rightLegAngle <= 90) stepFlag = 0;
      }
      updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度

      stateModel = STATE_MODEL_MOVING; // 设置状态为移动
    }
    break;

    case ACTION_MODEL_RIGHT: // 右转
    {
      if (stepFlag == 0)
      {
        leftHandAngle -= moveSpeed;
        rightLegAngle += moveSpeed;
        
        if (leftHandAngle <= THERSHOLD_ANGLE) stepFlag = 1;
      }
      else if (stepFlag == 1)
      {
        leftLegAngle += moveSpeed;
        rightHandAngle -= moveSpeed;
        
        if (leftLegAngle >= (180 - THERSHOLD_ANGLE)) stepFlag = 2;
      }
      else if (stepFlag == 2)
      {
        leftHandAngle += moveSpeed;
        rightLegAngle -= moveSpeed;
        
        if (leftHandAngle >= 90) stepFlag = 3;
      }
      else if (stepFlag == 3)
      {
        leftLegAngle -= moveSpeed;
        rightHandAngle += moveSpeed;
        
        if (leftLegAngle <= 90) stepFlag = 0;
      }
      updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度

      stateModel = STATE_MODEL_MOVING; // 设置状态为移动
    }
    break;

    case ACTION_MODEL_BACKWARD: // 后退
    {
      if (stepFlag == 0)
      {
        leftHandAngle += moveSpeed;
        rightLegAngle += moveSpeed;
        
        if (leftHandAngle >= (180 - THERSHOLD_ANGLE)) stepFlag = 1;
      }
      else if (stepFlag == 1)
      {
        rightHandAngle -= moveSpeed;
        leftLegAngle -= moveSpeed;
        
        if (rightHandAngle <= THERSHOLD_ANGLE) stepFlag = 2;
      }
      else if (stepFlag == 2)
      {
        leftHandAngle -= moveSpeed;
        rightLegAngle -= moveSpeed;
        
        if (leftHandAngle <= 90) stepFlag = 3;
      }
      else if (stepFlag == 3)
      {
        rightHandAngle += moveSpeed;
        leftLegAngle += moveSpeed;
        
        if (rightHandAngle >= (180 - THERSHOLD_ANGLE)) stepFlag = 4;
      }
      else if (stepFlag == 4)
      {
        leftHandAngle -= moveSpeed;
        rightLegAngle -= moveSpeed;
        
        if (leftHandAngle <= THERSHOLD_ANGLE) stepFlag = 5;
      }
      else if (stepFlag == 5)
      {
        rightHandAngle -= moveSpeed;
        leftLegAngle -= moveSpeed;
        
        if (rightHandAngle <= 90) stepFlag = 6;
      }
      else if (stepFlag == 6)
      {
        leftHandAngle += moveSpeed;
        rightLegAngle += moveSpeed;
        
        if (leftHandAngle >= 90) stepFlag = 0;
      }
      updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度

      stateModel = STATE_MODEL_MOVING; // 设置状态为移动
    }
    break;

    case ACTION_MODEL_STANDUP: // 站立
    {
      standUp(); // 调用站立函数

      actionModel = ACTION_MODEL_NONE;
      stateModel = STATE_MODEL_STANDUP; // 设置状态为站立
    }
    break;

    case ACTION_MODEL_GETDOWN: // 趴下
    {
      getDown(); // 调用趴下函数

      actionModel = ACTION_MODEL_NONE;
      stateModel = STATE_MODEL_GETDOWN; // 设置状态为趴下
    }
    break;

    case ACTION_MODEL_SITDOWN: // 坐下
    {
      sitDown(); // 调用坐下函数

      actionModel = ACTION_MODEL_NONE;
      stateModel = STATE_MODEL_SITDOWN; // 设置状态为坐下
    }
    break;

    case ACTION_MODEL_STRETCH: // 伸懒腰
    {
      stretch(); // 调用伸懒腰函数

      actionModel = ACTION_MODEL_NONE;
      stateModel = STATE_MODEL_STANDUP; // 设置状态为站立
    }
    break;

    case ACTION_MODEL_HANDSHAKE: // 握手
    {
      if (stepFlag == 0)
      {
        rightHandAngle -= moveSpeed;
        
        if (rightHandAngle <= 15) stepFlag = 1;
      }
      else if (stepFlag == 1)
      {
        rightHandAngle += moveSpeed;
        
        if (rightHandAngle >= 50) stepFlag = 0;
      }
      updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度

      stateModel = STATE_MODEL_MOVING; // 设置状态为移动
    }
    break;

    case ACTION_MODEL_CRAWL: // 匍匐前进
    {
      if (stepFlag == 0)
      {
        leftHandAngle -= moveSpeed;
        
        if (leftHandAngle <= 60) stepFlag = 1;
      }
      else if (stepFlag == 1)
      {
        rightHandAngle += moveSpeed;
        
        if (rightHandAngle >= 140) stepFlag = 2;
      }
      else if (stepFlag == 2)
      {
        leftHandAngle += moveSpeed;
        
        if (leftHandAngle >= 100) stepFlag = 3;
      }
      else if (stepFlag == 3)
      {
        rightHandAngle -= moveSpeed;
        
        if (rightHandAngle <= 60) stepFlag = 4;
      }
      else if (stepFlag == 4)
      {
        leftHandAngle += moveSpeed;
        
        if (leftHandAngle >= 140) stepFlag = 5;
      }
      else if (stepFlag == 5)
      {
        rightHandAngle += moveSpeed;
        
        if (rightHandAngle >= 100) stepFlag = 0;
      }
      updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度

      stateModel = STATE_MODEL_MOVING; // 设置状态为移动
    }
    break;

    case ACTION_MODEL_LEFTRIGHT: // 左右摇晃
    {
      if (stepFlag == 0)
      {
        while ((abs(rightHandAngle - 20) > moveSpeed) || (abs(rightLegAngle - 20) > moveSpeed))
        {
          if (rightHandAngle < 20) rightHandAngle += moveSpeed;
          else if (rightHandAngle > 20) rightHandAngle -= moveSpeed;

          if (rightLegAngle < 20) rightLegAngle += moveSpeed;
          else if (rightLegAngle > 20) rightLegAngle -= moveSpeed;

          delay(10); // 延时，避免过快

          updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
        }
  
        stepFlag = 1;
      }
      else if (stepFlag == 1)
      {
        leftHandAngle -= moveSpeed;
        rightHandAngle += moveSpeed;
        leftLegAngle -= moveSpeed;
        rightLegAngle += moveSpeed;
        
        if (leftHandAngle <= 20) stepFlag = 2;
      }
      else if (stepFlag == 2)
      {
        leftHandAngle += moveSpeed;
        rightHandAngle -= moveSpeed;
        leftLegAngle += moveSpeed;
        rightLegAngle -= moveSpeed;
        
        if (leftHandAngle >= 90) stepFlag = 0;
      }
      updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度

      stateModel = STATE_MODEL_MOVING; // 设置状态为移动
    }
    break;

    case ACTION_MODEL_FRONTBACK: // 前后摇晃
    {
      if (stepFlag == 0)
      {
        while ((abs(leftLegAngle - 160) > moveSpeed) || (abs(rightLegAngle - 160) > moveSpeed))
        {
          if (leftLegAngle > 160) leftLegAngle -= moveSpeed;
          else if (leftLegAngle < 160) leftLegAngle += moveSpeed;

          if (rightLegAngle > 160) rightLegAngle -= moveSpeed;
          else if (rightLegAngle < 160) rightLegAngle += moveSpeed;

          delay(10); // 延时，避免过快

          updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
        }
  
        stepFlag = 1;
      }
      else if (stepFlag == 1)
      {
        leftHandAngle -= moveSpeed;
        rightHandAngle -= moveSpeed;
        leftLegAngle -= moveSpeed;
        rightLegAngle -= moveSpeed;
        
        if (leftHandAngle <= 20) stepFlag = 2;
      }
      else if (stepFlag == 2)
      {
        leftHandAngle += moveSpeed;
        rightHandAngle += moveSpeed;
        leftLegAngle += moveSpeed;
        rightLegAngle += moveSpeed;
        
        if (leftHandAngle >= 90) stepFlag = 0;
      }
      updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度

      stateModel = STATE_MODEL_MOVING; // 设置状态为移动
    }
    break;

    case ACTION_MODEL_HEADFWD:    // 向前顶
    {
      headForward();

      actionModel = ACTION_MODEL_NONE;
      stateModel = STATE_MODEL_STANDUP; // 设置状态为站立
    }
    break;

    case ACTION_MODEL_HEADBACK:   // 向后顶
    {
      headBackward();

      actionModel = ACTION_MODEL_NONE;
      stateModel = STATE_MODEL_STANDUP; // 设置状态为站立
    }
    break;

    default:
    stepFlag = 0;
    break;
  }

  // 空闲时把当前舵机角度值发送给 WeXCube 小程序页面
  if (wexConnectFlag && (actionModel == ACTION_MODEL_NONE)) {
    static int lastLightLevel = -1;
    static int lastMoveSpeed = -1;
    static int lastLeftHandAngle = -1;
    static int lastRightHandAngle = -1;
    static int lastLeftLegAngle = -1;
    static int lastRightLegAngle = -1;

    // 检查眼睛亮度是否有变化
    if (lastLightLevel != lightLevel) {
      lastLightLevel = lightLevel;
      wex_setValue(SLIDER_EYES_LEVEL_ID, lightLevel);
    }

    // 检查运动速度是否有变化
    if (lastMoveSpeed != moveSpeed) {
      lastMoveSpeed = moveSpeed;
      wex_setValue(SLIDER_MOVE_SPEED_ID, moveSpeed / 2);
    }

    // 检查舵机角度是否有变化
    if (lastLeftHandAngle != leftHandAngle) {
      lastLeftHandAngle = leftHandAngle;
      wex_setValue(SLIDER_LEFTHAND_ID, leftHandAngle);  // 左手舵机角度
    }
    if (lastRightHandAngle != rightHandAngle) {
      lastRightHandAngle = rightHandAngle;
      wex_setValue(SLIDER_RIGHTHAND_ID, rightHandAngle);  // 右手舵机角度
    }
    if (lastLeftLegAngle != leftLegAngle) {
      lastLeftLegAngle = leftLegAngle;
      wex_setValue(SLIDER_LEFTLEG_ID, leftLegAngle);  // 左脚舵机角度
    }
    if (lastRightLegAngle != rightLegAngle) {
      lastRightLegAngle = rightLegAngle;
      wex_setValue(SLIDER_RIGHTLEG_ID, rightLegAngle);  // 右脚舵机角度
    }
  }

  // 如果有按键被按下，则询问当前按键值
  if (wexConnectFlag && currentButtonId) {
    static wex_u32_t cnt = 0;
    cnt++;
    // 每 25 x 20 = 500ms 询问一次按键值
    if (cnt % 25 == 0)
      wex_askValue(currentButtonId);
  }

  // 定时器中断处理，1秒钟执行一次
  if (timerFlag) {
    timerFlag = false;

    // 反转 5 引脚电平，保持 5V 电源开启
    digitalWrite(5, !digitalRead(5));
    
    // 如果 WeXCube 连接成功则执行心跳包
    if (wexConnectFlag) {
      // 发送电池状态到 WeXCube
      if (hardwareVersion >= 0x0101)
        wex_setText(TEXT_BATTERY_ID, pcBatteryStatusStr[batteryStatus]); // 39 是电池状态控件 ID
      else
        wex_setText(TEXT_BATTERY_ID, ""); // 旧版本硬件不支持电池状态

      // 心跳包计数超过4次则认为 WeXCube 断开连接
      askHandshakeCnt++;
      if (askHandshakeCnt > 4) {
        askHandshakeCnt = 0;

        Serial.println("WeXCube disconnect");
        digitalWrite(WEX_LED_PIN, HIGH);

        wexConnectFlag = false;
        
        if (wexcubeCtrlFlag) {
          wexcubeCtrlFlag = false; // 停止 WeXCube 控制
          // 停止动作
          actionModel = ACTION_MODEL_NONE;
        }
      }
    }
  }
}

// WeXCube 处理
void wex_ble_loop() {
  const t_sWexCmd *psWexCmd = wex_process();
  switch (psWexCmd->eCmdType)
  {
    case eWexCmd_Connect:       		// WeXCube 连接指令
    {
      Serial.println("WeXCube connect");
      digitalWrite(WEX_LED_PIN, LOW);

      wexConnectFlag = true;

      // WeXCube连接成功则初始化设备页面
      char firmwareStr[10] = {0};
      sprintf(firmwareStr, "V%d.%d.%d", (firmwareVersion >> 16) & 0xFF, (firmwareVersion >> 8) & 0xFF, firmwareVersion & 0xFF);
      wex_setText(TEXT_VERSION_ID, firmwareStr); // 显示固件版本号
      char hardwareStr[10] = {0};
      sprintf(hardwareStr, "V%d.%d", (hardwareVersion >> 8) & 0xFF, hardwareVersion & 0xFF);
      wex_setText(TEXT_HARDWARE_ID, hardwareStr);  // 显示硬件版本号
      selectEyesColor(BUTTON_EYES_COLOR1_ID + lightColor); // 眼睛颜色
      wex_setValue(SLIDER_EYES_LEVEL_ID, lightLevel); // 眼睛亮度值
      selectEyesModel(BUTTON_EYES_MODE1_ID + lightModel); // 眼睛模式

      wex_setValue(SLIDER_LEFTHAND_ID, leftHandAngle);  // 左手舵机角度
      wex_setValue(SLIDER_RIGHTHAND_ID, rightHandAngle);  // 右手舵机角度
      wex_setValue(SLIDER_LEFTLEG_ID, leftLegAngle);  // 左脚舵机角度
      wex_setValue(SLIDER_RIGHTLEG_ID, rightLegAngle);  // 右脚舵机角度

      wex_setValue(SLIDER_MOVE_SPEED_ID, moveSpeed / 2); // 动作速度
    }
    break;

    case eWexCmd_Disconnect:        // WeXCube 断开连接指令
    {
      Serial.println("WeXCube disconnect");
      digitalWrite(WEX_LED_PIN, HIGH);

      askHandshakeCnt = 0;
      wexConnectFlag = false;

      if (wexcubeCtrlFlag) {
          wexcubeCtrlFlag = false; // 停止 WeXCube 控制
          // 停止动作
          actionModel = ACTION_MODEL_NONE;
      }
    }
    break;

    case eWexCmd_AskHandshake:      // WeXCube 请求握手指令
    {
      askHandshakeCnt = 0;
    }
    break;

    case eWexCmd_PopupDialog:       // 对话框回复
    {
      if (psWexCmd->ucValue)
      {
        // 校准复位
        leftHandOffset = 0;
        rightHandOffset = 0;
        leftLegOffset = 0;
        rightLegOffset = 0;

        // 保存舵机角度偏移
        saveOffsetsToFlash();
        wex_showPopup(eWexPopupType_Success, "校准复位成功");

        // 更新舵机角度
        leftHandAngle = 90;
        rightHandAngle = 90;
        leftLegAngle = 90;
        rightLegAngle = 90;
        updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);

        // 更新小程序滑块值
        wex_setValue(SLIDER_LEFTHAND_ID, leftHandAngle);
        wex_setValue(SLIDER_RIGHTHAND_ID, rightHandAngle);
        wex_setValue(SLIDER_LEFTLEG_ID, leftLegAngle);
        wex_setValue(SLIDER_RIGHTLEG_ID, rightLegAngle);

        stateModel = STATE_MODEL_STANDUP;
      }
    }
    break;

    case eWexCmd_Event:             // 控件事件触发
    {
      Serial.printf("WeXCube event, Ctrl ID is %d, Value is %d\n", psWexCmd->ucCtrlId, psWexCmd->ucValue);
      switch (psWexCmd->ucCtrlId)
      {
        case BUTTON_EYES_COLOR1_ID: // 眼睛颜色1
        case BUTTON_EYES_COLOR2_ID: // 眼睛颜色2
        case BUTTON_EYES_COLOR3_ID: // 眼睛颜色3
        case BUTTON_EYES_COLOR4_ID: // 眼睛颜色4
        case BUTTON_EYES_COLOR5_ID: // 眼睛颜色5
        case BUTTON_EYES_COLOR6_ID: // 眼睛颜色6
        {
          selectEyesColor(psWexCmd->ucCtrlId); // 选择眼睛颜色
        }
        break;

        case SLIDER_EYES_LEVEL_ID: // 眼睛亮度调节
        {
          lightLevel = psWexCmd->ucValue;
          Serial.printf("Eyes light level: %d\n", lightLevel);
          updateRGB(ledRGB[0], ledRGB[1], ledRGB[2], lightLevel);
        }
        break;

        case BUTTON_EYES_MODE1_ID: // 眼睛模式1
        case BUTTON_EYES_MODE2_ID: // 眼睛模式2
        case BUTTON_EYES_MODE3_ID: // 眼睛模式3
        {
          selectEyesModel(psWexCmd->ucCtrlId); // 选择眼睛模式
        }
        break;

        case SLIDER_LEFTHAND_ID:        // 左手舵机控制
        {
          leftHandAngle = psWexCmd->ucValue;
          updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
        }
        break;

        case SLIDER_RIGHTHAND_ID:      // 右手舵机控制
        {
          rightHandAngle = psWexCmd->ucValue;
          updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
        }
        break;

        case SLIDER_LEFTLEG_ID:       // 左脚舵机控制
        {
          leftLegAngle = psWexCmd->ucValue;
          updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
        }
        break;

        case SLIDER_RIGHTLEG_ID:      // 右脚舵机控制
        {
          rightLegAngle = psWexCmd->ucValue;
          updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
        }
        break;

        case BUTTON_LEFTHAND_MINUS_ID: // 左手舵机角度减小
        {
          if (psWexCmd->ucValue == 0)
          {
            currentButtonId = 0; // 按键松开，清除当前按键 ID
            break;
          }
          currentButtonId = BUTTON_LEFTHAND_MINUS_ID; // 记录当前按键 ID

          if (leftHandAngle > 0) leftHandAngle -= 1; // 减小角度
          updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
        }
        break;

        case BUTTON_LEFTHAND_PLUS_ID:  // 左手舵机角度增大
        {
          if (psWexCmd->ucValue == 0)
          {
            currentButtonId = 0; // 按键松开，清除当前按键 ID
            break;
          }
          currentButtonId = BUTTON_LEFTHAND_PLUS_ID; // 记录当前按键 ID

          if (leftHandAngle < 180) leftHandAngle += 1; // 增大角度
          updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
        }
        break;

        case BUTTON_RIGHTHAND_MINUS_ID: // 右手舵机角度减小
        {
          if (psWexCmd->ucValue == 0)
          {
            currentButtonId = 0; // 按键松开，清除当前按键 ID
            break;
          }
          currentButtonId = BUTTON_RIGHTHAND_MINUS_ID; // 记录当前按键 ID

          if (rightHandAngle > 0) rightHandAngle -= 1; // 减小角度
          updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
        }
        break;

        case BUTTON_RIGHTHAND_PLUS_ID:  // 右手舵机角度增大
        {
          if (psWexCmd->ucValue == 0)
          {
            currentButtonId = 0; // 按键松开，清除当前按键 ID
            break;
          }
          currentButtonId = BUTTON_RIGHTHAND_PLUS_ID; // 记录当前按键 ID

          if (rightHandAngle < 180) rightHandAngle += 1; // 增大角度
          updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
        }
        break;

        case BUTTON_LEFTLEG_MINUS_ID: // 左脚舵机角度减小
        {
          if (psWexCmd->ucValue == 0)
          {
            currentButtonId = 0; // 按键松开，清除当前按键 ID
            break;
          }
          currentButtonId = BUTTON_LEFTLEG_MINUS_ID; // 记录当前按键 ID

          if (leftLegAngle > 0) leftLegAngle -= 1; // 减小角度
          updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
        }
        break;

        case BUTTON_LEFTLEG_PLUS_ID:  // 左脚舵机角度增大
        {
          if (psWexCmd->ucValue == 0)
          {
            currentButtonId = 0; // 按键松开，清除当前按键 ID
            break;
          }
          currentButtonId = BUTTON_LEFTLEG_PLUS_ID; // 记录当前按键 ID

          if (leftLegAngle < 180) leftLegAngle += 1; // 增大角度
          updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
        }
        break;

        case BUTTON_RIGHTLEG_MINUS_ID: // 右脚舵机角度减小
        {
          if (psWexCmd->ucValue == 0)
          {
            currentButtonId = 0; // 按键松开，清除当前按键 ID
            break;
          }
          currentButtonId = BUTTON_RIGHTLEG_MINUS_ID; // 记录当前按键 ID

          if (rightLegAngle > 0) rightLegAngle -= 1; // 减小角度
          updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
        }
        break;

        case BUTTON_RIGHTLEG_PLUS_ID:  // 右脚舵机角度增大
        {
          if (psWexCmd->ucValue == 0)
          {
            currentButtonId = 0; // 按键松开，清除当前按键 ID
            break;
          }
          currentButtonId = BUTTON_RIGHTLEG_PLUS_ID; // 记录当前按键 ID

          if (rightLegAngle < 180) rightLegAngle += 1; // 增大角度
          updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
        }
        break;

        case BUTTON_CALI_RESET_ID:  // 校准复位
        {
          if (psWexCmd->ucValue)
          {
            // 弹出确认对话框
            wex_showPopup(eWexPopupType_Select, "确定要校准复位吗？");
          }
        }
        break;

        case BUTTON_CALI_SET_ID:  // 校准写入
        {
          if (psWexCmd->ucValue == 0) break;

          if (stateModel != STATE_MODEL_STANDUP)
          {
            // 不是站立状态，提示用户
            wex_showPopup(eWexPopupType_Error, "请先站立再操作");
          }
          else
          {
            if (abs(leftHandAngle - 90) > 15 || abs(rightHandAngle - 90) > 15 ||
                abs(leftLegAngle - 90) > 15 || abs(rightLegAngle - 90) > 15)
            {
              // 偏移角度过大，提示用户
              wex_showPopup(eWexPopupType_Error, "偏移角度过大");
            }
            else
            {
              // 校准参数
              leftHandOffset += (leftHandAngle - 90);
              rightHandOffset += (rightHandAngle - 90);
              leftLegOffset += (leftLegAngle - 90);
              rightLegOffset += (rightLegAngle - 90);

              // 保存舵机角度偏移
              saveOffsetsToFlash();
              wex_showPopup(eWexPopupType_Success, "校准写入成功");

              // 更新小程序滑块值
              leftHandAngle = 90;
              rightHandAngle = 90;
              leftLegAngle = 90;
              rightLegAngle = 90;
            }
          }
        }
        break;

        case BUTTON_FORWARD_ID:       // 前进
        {
          if (psWexCmd->ucValue)
          {
            wexcubeCtrlFlag = true; // WeXCube 控制标志
            actionModel = ACTION_MODEL_FORWARD;
            currentButtonId = BUTTON_FORWARD_ID; // 记录当前按键 ID
            standUp(); // 站立
          }
          else
          {
            wexcubeCtrlFlag = false;
            actionModel = ACTION_MODEL_NONE; // 停止
            currentButtonId = 0; // 清除当前按键 ID
          }
        }
        break;

        case BUTTON_LEFT_ID:          // 左转
        {
          if (psWexCmd->ucValue)
          {
            wexcubeCtrlFlag = true; // WeXCube 控制标志
            actionModel = ACTION_MODEL_LEFT;
            currentButtonId = BUTTON_LEFT_ID; // 记录当前按键 ID
            standUp(); // 站立
          }
          else
          {
            wexcubeCtrlFlag = false;
            actionModel = ACTION_MODEL_NONE; // 停止
            currentButtonId = 0; // 清除当前按键 ID
          }
        }
        break;

        case BUTTON_RIGHT_ID:         // 右转
        {
          if (psWexCmd->ucValue)
          {
            wexcubeCtrlFlag = true; // WeXCube 控制标志
            actionModel = ACTION_MODEL_RIGHT;
            currentButtonId = BUTTON_RIGHT_ID; // 记录当前按键 ID
            standUp(); // 站立
          }
          else
          {
            wexcubeCtrlFlag = false;
            actionModel = ACTION_MODEL_NONE; // 停止
            currentButtonId = 0; // 清除当前按键 ID
          }
        }
        break;

        case BUTTON_BACKWARD_ID:      // 后退
        {
          if (psWexCmd->ucValue)
          {
            wexcubeCtrlFlag = true; // WeXCube 控制标志
            actionModel = ACTION_MODEL_BACKWARD;
            currentButtonId = BUTTON_BACKWARD_ID; // 记录当前按键 ID
            standUp(); // 站立
          }
          else
          {
            wexcubeCtrlFlag = false;
            actionModel = ACTION_MODEL_NONE; // 停止
            currentButtonId = 0; // 清除当前按键 ID
          }
        }
        break;

        case BUTTON_STANDUP_ID:       // 站立
        {
          if (psWexCmd->ucValue) actionModel = ACTION_MODEL_STANDUP;
        }
        break;

        case BUTTON_GETDOWN_ID:       // 趴下
        {
          if (psWexCmd->ucValue) actionModel = ACTION_MODEL_GETDOWN;
        }
        break;

        case BUTTON_HANDSHAKE_ID:     // 握手
        {
          if (psWexCmd->ucValue)
          {
            wexcubeCtrlFlag = true; // WeXCube 控制标志
            actionModel = ACTION_MODEL_HANDSHAKE;
            currentButtonId = BUTTON_HANDSHAKE_ID; // 记录当前按键 ID
            sitDown(); // 坐下
          }
          else
          {
            wexcubeCtrlFlag = false;
            actionModel = ACTION_MODEL_NONE; // 停止
            currentButtonId = 0; // 清除当前按键 ID
          }
        }
        break;

        case BUTTON_CRAWL_ID:         // 匍匐前进
        {
          if (psWexCmd->ucValue)
          {
            wexcubeCtrlFlag = true; // WeXCube 控制标志
            actionModel = ACTION_MODEL_CRAWL;
            currentButtonId = BUTTON_CRAWL_ID; // 记录当前按键 ID
            sitDown(); // 坐下
          }
          else
          {
            wexcubeCtrlFlag = false;
            actionModel = ACTION_MODEL_NONE; // 停止
            currentButtonId = 0; // 清除当前按键 ID
          }
        }
        break;

        case BUTTON_LEFTRIGHT_ID:     // 左右摇晃
        {
          if (psWexCmd->ucValue)
          {
            wexcubeCtrlFlag = true; // WeXCube 控制标志
            actionModel = ACTION_MODEL_LEFTRIGHT;
            currentButtonId = BUTTON_LEFTRIGHT_ID; // 记录当前按键 ID
            standUp(); // 站立
          }
          else
          {
            wexcubeCtrlFlag = false;
            actionModel = ACTION_MODEL_NONE; // 停止
            currentButtonId = 0; // 清除当前按键 ID
          }
        }
        break;

        case BUTTON_FRONTBACK_ID:        // 前后摇晃
        {
          if (psWexCmd->ucValue)
          {
            wexcubeCtrlFlag = true; // WeXCube 控制标志
            actionModel = ACTION_MODEL_FRONTBACK;
            currentButtonId = BUTTON_FRONTBACK_ID; // 记录当前按键 ID
            standUp(); // 站立
          }
          else
          {
            wexcubeCtrlFlag = false;
            actionModel = ACTION_MODEL_NONE; // 停止
            currentButtonId = 0; // 清除当前按键 ID
          }
        } 
        break;

        case BUTTON_SITDOWN_ID:       // 坐下
        {
          if (psWexCmd->ucValue) actionModel = ACTION_MODEL_SITDOWN;
        }
        break;

        case BUTTON_STRETCH_ID:     // 伸懒腰
        {
          if (psWexCmd->ucValue) actionModel = ACTION_MODEL_STRETCH;
        }
        break;

        case BUTTON_HEADFWD_ID:       // 前顶
        {
          if (psWexCmd->ucValue) actionModel = ACTION_MODEL_HEADFWD;
        }
        break;

        case BUTTON_HEADBACK_ID:      // 后顶
        {
          if (psWexCmd->ucValue) actionModel = ACTION_MODEL_HEADBACK;
        }
        break;

        case SLIDER_MOVE_SPEED_ID: // 动作速度调节
        {
          if (psWexCmd->ucValue == 0) 
          {
            moveSpeed = 2; // 最小速度
            wex_setValue(SLIDER_MOVE_SPEED_ID, 1); // 设置滑动条值为 1
          }
          else 
          {
            moveSpeed = psWexCmd->ucValue * 2;
          }
          Serial.printf("Move speed: %d\n", moveSpeed);
        }
        break;

        default:
        break;
      }
    }
    break;

    case eWexCmd_Value:          // 控件值, wex_askValue 的回应
    {
      Serial.printf("WeXCube value, Ctrl ID is %d, Value is %d\n", psWexCmd->ucCtrlId, psWexCmd->ucValue);
      switch (psWexCmd->ucCtrlId)
      {
        case SLIDER_LEFTHAND_ID:        // 左手舵机控制
        case SLIDER_RIGHTHAND_ID:      // 右手舵机控制
        case SLIDER_LEFTLEG_ID:       // 左脚舵机控制
        case SLIDER_RIGHTLEG_ID:      // 右脚舵机控制
        {
          if (psWexCmd->ucValue == 0) // 按键抬起了
          {
            wexcubeCtrlFlag = false;
            actionModel = ACTION_MODEL_NONE; // 停止
            currentButtonId = 0; // 按键抬起，清除当前按键 ID
          }
        }
        break;

        case BUTTON_LEFTHAND_MINUS_ID: // 左手舵机角度减小
          if (psWexCmd->ucValue)
          {
            if (leftHandAngle)
            {
              leftHandAngle -= 1;
              updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
            }
          }
          else
            currentButtonId = 0; // 按键抬起，清除当前按键 ID
        break;

        case BUTTON_LEFTHAND_PLUS_ID:  // 左手舵机角度增大
          if (psWexCmd->ucValue)
          {
            if(leftHandAngle < 180)
            {
              leftHandAngle += 1;
              updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
            }
          }
          else
            currentButtonId = 0; // 按键抬起，清除当前按键 ID
        break;

        case BUTTON_RIGHTHAND_MINUS_ID: // 右手舵机角度减小
          if (psWexCmd->ucValue)
          {
            if(rightHandAngle)
            {
              rightHandAngle -= 1;
              updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
            }
          }
          else
            currentButtonId = 0; // 按键抬起，清除当前按键 ID
        break;

        case BUTTON_RIGHTHAND_PLUS_ID:  // 右手舵机角度增大
          if (psWexCmd->ucValue)
          {
            if(rightHandAngle < 180)
            {
              rightHandAngle += 1;
              updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
            }
          }
          else
            currentButtonId = 0; // 按键抬起，清除当前按键 ID
        break;

        case BUTTON_LEFTLEG_MINUS_ID: // 左脚舵机角度减小
          if (psWexCmd->ucValue)
          {
            if(leftLegAngle)
            {
              leftLegAngle -= 1;
              updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
            }
          }
          else
            currentButtonId = 0; // 按键抬起，清除当前按键 ID
        break;

        case BUTTON_LEFTLEG_PLUS_ID:  // 左脚舵机角度增大
          if (psWexCmd->ucValue)
          {
            if(leftLegAngle < 180)
            {
              leftLegAngle += 1;
              updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
            }
          }
          else
            currentButtonId = 0; // 按键抬起，清除当前按键 ID
        break;

        case BUTTON_RIGHTLEG_MINUS_ID: // 右脚舵机角度减小
          if (psWexCmd->ucValue)
          {
            if(rightLegAngle)
            {
              rightLegAngle -= 1;
              updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
            }
          }
          else
            currentButtonId = 0; // 按键抬起，清除当前按键 ID
        break;

        case BUTTON_RIGHTLEG_PLUS_ID:  // 右脚舵机角度增大
          if (psWexCmd->ucValue)
          {
            if(rightLegAngle < 180)
            {
              rightLegAngle += 1;
              updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
            }
          }
          else
            currentButtonId = 0; // 按键抬起，清除当前按键 ID
        break;

        default:
        break;
      }
    }
    break;

    case eWexCmd_BackRGB:
    {
      switch (psWexCmd->ucCtrlId)
      {
        case BUTTON_EYES_COLOR1_ID: // 眼睛颜色1
        case BUTTON_EYES_COLOR2_ID: // 眼睛颜色2
        case BUTTON_EYES_COLOR3_ID: // 眼睛颜色3
        case BUTTON_EYES_COLOR4_ID: // 眼睛颜色4
        case BUTTON_EYES_COLOR5_ID: // 眼睛颜色5
        case BUTTON_EYES_COLOR6_ID: // 眼睛颜色6
        {
          // WeXCube 颜色值返回
          ledRGB[0] = psWexCmd->sColor.ucR; // 红色分量
          ledRGB[1] = psWexCmd->sColor.ucG; // 绿色分量
          ledRGB[2] = psWexCmd->sColor.ucB; // 蓝色分量
          updateRGB(ledRGB[0], ledRGB[1], ledRGB[2], lightLevel);
          Serial.printf("Eyes light color: %d, %d, %d\n", ledRGB[0], ledRGB[1], ledRGB[2]);
        }
        break;
      
      default:
        break;
      }
    }
    break;

    case eWexCmd_LinkDog_Eyes:  // 灵动狗眼睛控制
    {
      lightModel = LIGHT_MODEL_NORMAL; // 设置眼睛模式为正常模式
      lightLevel = psWexCmd->sLinkDog.ucEyesLevel;
      ledRGB[0] = psWexCmd->sLinkDog.ucEyesRed;
      ledRGB[1] = psWexCmd->sLinkDog.ucEyesGreen;
      ledRGB[2] = psWexCmd->sLinkDog.ucEyesBlue;
      updateRGB(ledRGB[0], ledRGB[1], ledRGB[2], lightLevel);
    }
    break;

    case eWexCmd_LinkDog_Limb:  // 灵动狗四肢控制
    {
      leftHandAngle = psWexCmd->sLinkDog.ucLeftHandAngle;
      rightHandAngle = psWexCmd->sLinkDog.ucRightHandAngle;
      leftLegAngle = psWexCmd->sLinkDog.ucLeftLegAngle;
      rightLegAngle = psWexCmd->sLinkDog.ucRightLegAngle;
      updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
    }
    break;

    case eWexCmd_LinkDog_Eyes_Limb:  // 灵动狗眼睛及四肢控制
    {
      lightModel = LIGHT_MODEL_NORMAL; // 设置眼睛模式为正常模式
      lightLevel = psWexCmd->sLinkDog.ucEyesLevel;
      ledRGB[0] = psWexCmd->sLinkDog.ucEyesRed;
      ledRGB[1] = psWexCmd->sLinkDog.ucEyesGreen;
      ledRGB[2] = psWexCmd->sLinkDog.ucEyesBlue;
      updateRGB(ledRGB[0], ledRGB[1], ledRGB[2], lightLevel);
      
      leftHandAngle = psWexCmd->sLinkDog.ucLeftHandAngle;
      rightHandAngle = psWexCmd->sLinkDog.ucRightHandAngle;
      leftLegAngle = psWexCmd->sLinkDog.ucLeftLegAngle;
      rightLegAngle = psWexCmd->sLinkDog.ucRightLegAngle;
      updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  
    }
    break;

    default:
    break;
  }
}


/**
 * @brief RGB 颜色、亮度更新
 * @param ucRed 红色分量
 * @param ucGreen 绿色分量
 * @param ucBlue 蓝色分量
 * @param ucLevel 亮度
 * @retval None
 */
void updateRGB(wex_u8_t ucRed, wex_u8_t ucGreen, wex_u8_t ucBlue, wex_u8_t ucLevel)
{
  for (int i = 0; i < EYES_NUM; i++)
  {
    strip.setPixelColor(i, strip.Color(ucRed, ucGreen, ucBlue));
  }
  strip.setBrightness(ucLevel * 2.55f);
  strip.show();
}


/**
 * @brief 舵机更新
 * @param ucLeftHand 左手舵机角度
 * @param ucRightHand 右手舵机角度
 * @param ucLeftLeg 左脚舵机角度
 * @param ucRightLeg 右脚舵机角度
 * @retval None
 */
void updateServo(wex_u8_t ucLeftHand, wex_u8_t ucRightHand, wex_u8_t ucLeftLeg, wex_u8_t ucRightLeg)
{
  // 根据偏移量修正舵机角度
  int leftHand = ucLeftHand + leftHandOffset;
  int rightHand  = ucRightHand + rightHandOffset;
  int leftLeg  = ucLeftLeg + leftLegOffset;
  int rightLeg  = ucRightLeg + rightLegOffset;

  leftHand = constrain(leftHand, 0, 180); // 限制角度范围
  rightHand = constrain(rightHand, 0, 180); // 限制角度范围
  leftLeg = constrain(leftLeg, 0, 180); // 限制角度范围
  rightLeg = constrain(rightLeg, 0, 180); // 限制角度范围

  // 把舵机角度统一化，0为最前，180为最后
  rightHand = 180 - rightHand; // 右手舵机角度反转
  rightLeg = 180 - rightLeg; // 右脚舵机角度反转

  leftHandServo.write(leftHand);  // 设置左手舵机角度
  rightHandServo.write(rightHand);  // 设置右手舵机角度
  leftLegServo.write(leftLeg);  // 设置左脚舵机角度
  rightLegServo.write(rightLeg);  // 设置右脚舵机角度
}


// 选择眼睛颜色
void selectEyesColor(wex_u8_t id)
{
  wex_askBackRGB(id);
  lightColor = id - BUTTON_EYES_COLOR1_ID;
  // 设置选中颜色的按钮文本为 "✓"，其他按钮文本清空
  wex_setText(BUTTON_EYES_COLOR1_ID, BUTTON_EYES_COLOR1_ID == id ? "✓" : "");
  wex_setText(BUTTON_EYES_COLOR2_ID, BUTTON_EYES_COLOR2_ID == id ? "✓" : "");
  wex_setText(BUTTON_EYES_COLOR3_ID, BUTTON_EYES_COLOR3_ID == id ? "✓" : "");
  wex_setText(BUTTON_EYES_COLOR4_ID, BUTTON_EYES_COLOR4_ID == id ? "✓" : "");
  wex_setText(BUTTON_EYES_COLOR5_ID, BUTTON_EYES_COLOR5_ID == id ? "✓" : "");
  wex_setText(BUTTON_EYES_COLOR6_ID, BUTTON_EYES_COLOR6_ID == id ? "✓" : "");
}


// 选择眼睛模式
void selectEyesModel(wex_u8_t id)
{
  lightModel = id - BUTTON_EYES_MODE1_ID; // 0 - 正常，1 - 呼吸，2 - 彩灯
  // 设置选中模式的按钮文本为 "✓"，其他按钮文本清空
  wex_setText(BUTTON_EYES_MODE1_ID, BUTTON_EYES_MODE1_ID == id ? "✓" : "");
  wex_setText(BUTTON_EYES_MODE2_ID, BUTTON_EYES_MODE2_ID == id ? "✓" : "");
  wex_setText(BUTTON_EYES_MODE3_ID, BUTTON_EYES_MODE3_ID == id ? "✓" : "");
}


// 站立
void standUp()
{
  while ((abs(leftLegAngle - 90) > 5) || (abs(rightLegAngle - 90) > 5))
  {
    if (leftLegAngle > 90) leftLegAngle -= 3;
    else if (leftLegAngle < 90) leftLegAngle += 3;

    if (rightLegAngle > 90) rightLegAngle -= 3;
    else if (rightLegAngle < 90) rightLegAngle += 3;

    delay(20); // 延时，避免过快

    updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
  }
  leftLegAngle = 90;
  rightLegAngle = 90;
  updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
  
  while ((abs(leftHandAngle - 90) > 5) || (abs(rightHandAngle - 90) > 5))
  {
    if (leftHandAngle > 90) leftHandAngle -= 5;
    else if (leftHandAngle < 90) leftHandAngle += 5;

    if (rightHandAngle > 90) rightHandAngle -= 5;
    else if (rightHandAngle < 90) rightHandAngle += 5;

    delay(20); // 延时，避免过快
    
    updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
  }
  leftHandAngle = 90;
  rightHandAngle = 90;
  updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
}

// 趴下
void getDown()
{
  while ((abs(leftLegAngle - 15) > 5) || (abs(rightLegAngle - 15) > 5))
  {
    if (leftLegAngle > 15) leftLegAngle -= 5;
    else if (leftLegAngle < 15) leftLegAngle += 5;

    if (rightLegAngle > 15) rightLegAngle -= 5;
    else if (rightLegAngle < 15) rightLegAngle += 5;

    delay(20); // 延时，避免过快

    updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
  }
  leftLegAngle = 15;
  rightLegAngle = 15;
  updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
  
  while ((abs(leftHandAngle - 15) > 5) || (abs(rightHandAngle - 15) > 5))
  {
    if (leftHandAngle > 15) leftHandAngle -= 5;
    else if (leftHandAngle < 15) leftHandAngle += 5;

    if (rightHandAngle > 15) rightHandAngle -= 5;
    else if (rightHandAngle < 15) rightHandAngle += 5;

    delay(20); // 延时，避免过快
    
    updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
  }
  leftHandAngle = 15;
  rightHandAngle = 15;
  updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
}

// 坐下
void sitDown()
{
  while ((abs(leftLegAngle - 40) > 5) || (abs(rightLegAngle - 40) > 5))
  {
    if (leftLegAngle > 40) leftLegAngle -= 3;
    else if (leftLegAngle < 40) leftLegAngle += 3;

    if (rightLegAngle > 40) rightLegAngle -= 3;
    else if (rightLegAngle < 40) rightLegAngle += 3;

    delay(20); // 延时，避免过快

    updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
  }
  leftLegAngle = 40;
  rightLegAngle = 40;
  updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
  
  while ((abs(leftHandAngle - 100) > 5) || (abs(rightHandAngle - 100) > 5))
  {
    if (leftHandAngle > 100) leftHandAngle -= 5;
    else if (leftHandAngle < 100) leftHandAngle += 5;

    if (rightHandAngle > 100) rightHandAngle -= 5;
    else if (rightHandAngle < 100) rightHandAngle += 5;

    delay(20); // 延时，避免过快
    
    updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
  }
  leftHandAngle = 100;
  rightHandAngle = 100;
  updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
}

// 伸懒腰
void stretch()
{
  standUp(); // 先站立

  // 避免长时间未处理蓝牙接收信息，导致 WeXCube 断开连接
  wex_process();
  
  while (leftHandAngle >= 15)
  {
    leftHandAngle -= 3;
    rightHandAngle -= 3;
    leftLegAngle -= 1;
    rightLegAngle -= 1;

    delay(20); // 延时，避免过快

    updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
  }

  // 避免长时间未处理蓝牙接收信息，导致 WeXCube 断开连接
  wex_process();

  int n = 2;
  while (n--)
  {
    while (leftHandAngle <= 25)
    {
      leftHandAngle += 1;
      rightHandAngle += 1;

      delay(20); // 延时，避免过快

      updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
    }

    while (leftHandAngle >= 15)
    {
      leftHandAngle -= 2;
      rightHandAngle -= 2;

      delay(20); // 延时，避免过快

      updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
    }
  }

  // 避免长时间未处理蓝牙接收信息，导致 WeXCube 断开连接
  wex_process();
  
  while (leftLegAngle <= 165)
  {
    leftHandAngle += 3;
    rightHandAngle += 3;
    leftLegAngle += 3;
    rightLegAngle += 3;

    delay(20); // 延时，避免过快
    
    updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
  }

  // 避免长时间未处理蓝牙接收信息，导致 WeXCube 断开连接
  wex_process();

  n = 2;
  while (n--)
  {
    while (leftLegAngle >= 155)
    {
      leftLegAngle -= 1;
      rightLegAngle -= 1;

      delay(20); // 延时，避免过快

      updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
    }

    while (leftLegAngle <= 165)
    {
      leftLegAngle += 2;
      rightLegAngle += 2;

      delay(20); // 延时，避免过快

      updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
    }
  }

  // 避免长时间未处理蓝牙接收信息，导致 WeXCube 断开连接
  wex_process();

  while (leftHandAngle >= 90)
  {
    leftHandAngle -= 1;
    rightHandAngle -= 1;

    delay(20); // 延时，避免过快

    updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
  }

  // 避免长时间未处理蓝牙接收信息，导致 WeXCube 断开连接
  wex_process();

  while (leftLegAngle >= 90)
  {
    leftLegAngle -= 3;
    rightLegAngle -= 3;

    delay(20); // 延时，避免过快

    updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);  // 更新舵机角度
  }

  actionModel = ACTION_MODEL_NONE; // 停止动作
}

// 前顶
void headForward()
{
  standUp();
  leftHandAngle = 140;
  rightHandAngle = 140;
  leftLegAngle = 150;
  rightLegAngle = 150;
  updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
  delay(200);
  leftHandAngle = 90;
  rightHandAngle = 90;
  updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
  delay(100);
  leftLegAngle = 90;
  rightLegAngle = 90;
  updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
  delay(100);
}

// 后顶
void headBackward()
{
  standUp();
  leftHandAngle = 30;
  rightHandAngle = 30;
  leftLegAngle = 40;
  rightLegAngle = 40;
  updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
  delay(200);
  leftLegAngle = 90;
  rightLegAngle = 90;
  updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
  delay(100);
  leftHandAngle = 90;
  rightHandAngle = 90;
  updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
  delay(100);
}

// 保存舵机偏移量到 Flash
void saveOffsetsToFlash() {
  preferences.begin("servo", false);
  preferences.putInt("leftHandOffset", leftHandOffset);
  preferences.putInt("rightHandOffset", rightHandOffset);
  preferences.putInt("leftLegOffset", leftLegOffset);
  preferences.putInt("rightLegOffset", rightLegOffset);
  preferences.end();
}